#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  HTMotor)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorNormal, PIDControl)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorNormal, PIDControl)
#pragma config(Motor,  mtr_S1_C4_1,     motorH,        tmotorNone, openLoop)
#pragma config(Motor,  mtr_S1_C4_2,     motorI,        tmotorNone, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
// *!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void initializeRobot()
{
  // servo[servo1] = 0;
  // servo[servo2] = 0;
  // servo[servo3] = 0;
  // servo[servo4] = 0;
  // servo[servo5] = 0;
}
#include "JoystickDriver.c"
#include "SourceCode4.c"
// #include "Recorder1.c"
#include "Recorder2.c"
task main()
{
  waitForStart();
  initializeRobot();
  int x;
  int y;
  nMotorEncoder[motorD] = 0;
  nMotorEncoder[motorE] = 0;

  int Z = 10;
  while(true)                            // Infinite loop:
  {
    // waitForStart();
    getJoystickSettings(joystick);

    if(abs(joystick.joy1_y2) > Z)   // If the right analog stick's Y-axis readings are either above or below the Q:
    {
      motor[motorE] = ((joystick.joy1_y2));         // Motor D is assigned a power level equal to the right analog stick's Y-axis reading.
    }
    else                                    // Else if the readings are within the Q:
    {
      motor[motorE] = 0;                        // Motor D is stopped with a power level of 0.
    }


    if(abs(joystick.joy1_y1) > Z)   // If the left analog stick's Y-axis readings are either above or below the Q:
    {
      motor[motorD] = ((joystick.joy1_y1));         // Motor E is assigned a power level equal to the left analog stick's Y-axis reading.
    }
    else                                    // Else if the readings are within the Q:
    {
      motor[motorD] = 0;                        // Motor E is stopped with a power level of 0.
    }

    if(joy2Btn(6))
    {
      motor[motorG] = -100;
    }
    if(joy2Btn(5))
    {
      motor[motorG] = 0;
    }

    if(joy1Btn(1))
    {
      servo[servo1] = 243;
      servo[servo2] = 0;
    }
    if(joy1Btn(2))
    {
      servo[servo1] = 130;
      servo[servo2] = 120;
    }
    if(joy2Btn(9))
    {
      servo[servo3] = 38;
      servo[servo4] = 255;
      servo[servo5] = 103;
    }
     // if(joy2Btn(4))
    // {
    // wait1Msec(200);
    // meep();
    // }
    if(joy2Btn(10))
    {
      servo[servo3] = 66;
      servo[servo4] = 23;
      servo[servo5] = 87;
    }

    if(joy2Btn(11) || joy2Btn(12))
    {
      if(joy2Btn(11))
        motor[motorH] = 100;
      if(joy2Btn(12))
        motor[motorH] = -100;
    }
    else
    {
      motor[motorH] = 0;
    }


    if(joy2Btn(8) || joy2Btn(7))
    {
      if(joy2Btn(8))
        motor[motorF] = 100;
      if(joy2Btn(7))
        motor[motorF] = -100;
    }
    else
    {
      motor[motorF] = 0;
    }
  }
}
